// Filename                : throttle.c
//
// Functional Description  : Throttle voltage support functions
//----------------------------------------------------------------------------

#include "DRV922xx_trapezoidal_sensored.h"
#include "message_protocol_application.h"

#define ADC_PWR2   					10   									/*!< log2(max ADC Value + 1) - for throttle PWM calculation */
void filterThrottleReduceThr(void)
{
	if (AppThrottleVoltageFiltered <= 8)
	{
		AppThrottleVoltageFiltered = 0;
	}
	else
	{
		AppThrottleVoltageFiltered -= 8;
	}
}
void filterThrottleLogicLowCurrent(void)
{
	// Need slow voltage filter for increasing throttle. Decrease more aggressively
	if (AppThrottleVoltageFiltered < AppThrottleVoltage)
	{
		AppThrottleVoltageFiltered++;
	
		if (AppThrottleVoltageFiltered > 900) 
		{
			AppThrottleVoltageFiltered = 900;
		}
	}
	else if (AppThrottleVoltageFiltered > AppThrottleVoltage)
	{
		filterThrottleReduceThr();
	}
	else 
	{
		AppThrottleVoltageFiltered = AppThrottleVoltage;
	}
	// Clear this flag
//	update_pwm_flag = 0;
}
void filterThrottleLogicMidCurrent(void)
{
	// can't increase throttle, only decrease
	if (AppThrottleVoltageFiltered > AppThrottleVoltage)
	{
		filterThrottleReduceThr();
	}
}
void filterThrottleImon(void)
{	
	static unsigned int current_max = 0;	
    
    if (AppCurrent > current_max) {
   		current_max = AppCurrent;
   	}
   	#if MSP_CONTROL
		if (AppThrottleVoltage > AppThrottleVoltage_Reference) 
		{
			AppThrottleVoltage = AppThrottleVoltage_Reference;
		}
	#endif
	if (AppCurrent <= AppIfilterMid)														// If we are over the mid current threshold, hold duty cycle steady
	{
//		if (update_pwm_flag)
//		{
		filterThrottleLogicLowCurrent();
//		}
	}
	else
	{
		filterThrottleLogicMidCurrent();
	}
}	
void set_duty(void)
{	
	unsigned short pwm_duty_low;
	if (complementary_pwm)
	{
	    // Apply the dead time to the low side PWM signal
	    if(pwm_duty == 0)
	        pwm_duty = 1;
	    if(pwm_duty > (TBCCR0 - AppDeadTime))
	        pwm_duty_low = TBCCR0;
	    else
	        pwm_duty_low = pwm_duty + AppDeadTime;
	  
	     // Note the order doesn't matter since the updated occurs 
	     // in the interrupt before TBR==TBCL0 for next cycle
	    TBCCR1 = TBCCR3 = TBCCR5 = pwm_duty;
	    TBCCR2 = TBCCR4 = TBCCR6 = pwm_duty_low;
	}
	else
		TBCCR1 = TBCCR3 = TBCCR5 = pwm_duty;
								
	// Update the PWM leading and trailing edge interrupts for the next switching cycle
	TA3CCR1 = CCR_MAX*2 - pwm_duty;									// leading PWM edge
	if (pwm_duty < TA2_CCR_3US_COUNTS)								// falling PWM edge. Make sure don't back up off end.
		TA3CCR2 = 1; //0;
	else
		TA3CCR2 = pwm_duty - TA2_CCR_3US_COUNTS;
}
unsigned int throttleGetState(void)
{
	if (AppThrottleVoltageFiltered <= 215)
	{
		return THROTTLE_OFF;
	}
	return THROTTLE_ON;
}
void throttle_filter(void)
{
	// ------------------------------------
    // Filter the throttle
    // ------------------------------------	
    filterThrottleImon();
				
    // ------------------------------------
    // Calculate new PWM duty cycle
    // ------------------------------------
	pwm_duty = throttle_voltage_to_pwm(AppThrottleVoltageFiltered);
	
	// ------------------------------------
    // Set new duty cycle
    // ------------------------------------	
    set_duty();
	
	// If the duty cycle is 0, then do not drive the LS FETs
//	if (throttleGetState() == THROTTLE_OFF)
//	{
//		P2OUT = led;
//		pwm_disable();
//	}
//	else if (update_commutator)
//	{
//		pwm_enable(comm_tbl_index);
//		update_commutator = 0;
//	}
}
/*
 *  \fn void sample_throttle(void)
 *  \brief Samples throttle voltage, filters and stores result in \a throttle_v_filtered and sets new PWM duty.
 */
void sample_throttle_setup(void)
{
	drv922xxAdcStartAndDelay(ADC_CHANNEL_SPEEDIN);
}
void sample_throttle(void)
{
 //   P1OUT |= BIT5;
    
    // ------------------------------------
    // Read the Throttle voltage
    // ------------------------------------
    sample_throttle_setup();
    throttle_voltage_high = drv922xxSpiRead(SPI_ADC1_READ);
    throttle_voltage_low = drv922xxSpiRead(SPI_ADC2_READ);
    AppThrottleVoltage = ((throttle_voltage_high & 0x3) << 8) | throttle_voltage_low;
#if MSP_CONTROL
    AppThrottleVoltage = AppThrottleVoltage_Reference;
#endif
//	__delay_cycles(25);

//	P1OUT &= ~BIT5;
}
/*
 *  \fn unsigned short throttle_voltage_to_pwm(unsigned int throttle_voltage_f2)
 *
 *  \brief Converts the throttle voltage (in ADC counts) to a PWM duty cycle (in CCR counts)
 *  \warn Be sure to set compiler optimization to 2 to produce inline code (Release build by default)
 * 
 *  \param throttle_voltage_f2 Voltage in ADC counts (10b)
 * 
 *  Assume that full the range of the ADC is a power of 2.
 *  We will adjust the voltage to pwm by following equation
 *  pwm_duty = ((voltage * TBCCR0) >> log2(ADC full range))
 */
//static inline unsigned int throttle_voltage_to_pwm(unsigned int throttle_voltage_f2)
unsigned int throttle_voltage_to_pwm(unsigned int throttle_voltage_f2)
{
	unsigned short pw_ccr;
	unsigned short compensated_v; 					// New range should be from 213 - 767 counts [0.48V - 1.8V]
	unsigned short max_pw_ccr = CCR_MAX;

	// Compensated voltage for throttle (0.8V - 1.6V)
	if(throttle_voltage_f2 < 215) 	//Low no throttle
		compensated_v = 0; 
	else if(throttle_voltage_f2 >= 215 && throttle_voltage_f2 <665) //Range for Throttle start
		compensated_v = ((throttle_voltage_f2 - 215) * 2); 				
	else if(throttle_voltage_f2 >= 665 && throttle_voltage_f2 <=910) //Range for high throttle
		compensated_v = 900; //665-215=450 *2= 88% full throttle
	else if(throttle_voltage_f2 >910 && throttle_voltage_f2 <1024)  //Over Voltage on throttle
		compensated_v = 0;
	else
		compensated_v = 0;  //Should never get here	
//	if(throttle_voltage_f2 < 300) 	//Low no throttle *215 original 300 for POT
//		compensated_v = 0;
//	else if(throttle_voltage_f2 >= 300 && throttle_voltage_f2 <750) //Range for Throttle start
//		compensated_v = ((throttle_voltage_f2 - 300) * 2); 			//(throttle_voltage_f2 - 300)	
//	else if(throttle_voltage_f2 >= 750 && throttle_voltage_f2 <=1024) //Range for high throttle
//		compensated_v = 900; //750-300=450 *2= 88% full throttle
////	else if(throttle_voltage_f2 >840 && throttle_voltage_f2 <1024)  //Over Voltage on throttle
////		compensated_v = 650;
//	else
//		compensated_v = 900;
//		compensated_v = 650;  //Should never get here		
	// Multiply shifted throttle voltage * max CCR counts (for max resolution)
	MPY = compensated_v << (16-ADC_PWR2);
	OP2 = CCR_MAX;

	// Get result from upper 16b of product
	pw_ccr = RESHI;
	    
    // Check to see if complementary PWM is enabled so we know to apply dead-time for maximum pulse width for HS
	if (complementary_pwm)
		max_pw_ccr -= AppDeadTime;
		
	if (pw_ccr > CCR_MAX)
		pw_ccr = max_pw_ccr;
	
    return pw_ccr;
}
void throttle_reset(void)
{
	AppThrottleVoltageFiltered = 0;
    AppThrottleVoltage = 0;  
    pwm_duty = 0; 
}
